#参考URL -> https://qiita.com/tibigame/items/61cecf86fc978628bfee
#参考図書 -> ポールのロボット・マニピュレータ
import numpy as np
import sympy as sym
sym.init_printing()
Pi = sym.S.Pi # 円周率
E = sym.S.Exp1 # 自然対数の底
I = sym.S.ImaginaryUnit # 虚数単位
#sympyの円周率の方を使うことをすすめる(こっちの方が量子化誤差が大きくなる.numpyも同様に大きい)
import math
pi = math.pi
# 角度変数
(J_1,J_2,J_3,J_4,J_5,J_6) = sym.symbols('J_1,J_2,J_3,J_4,J_5,J_6')
# リンクパラメータ
(a_1,a_2,a_3,d_4) = sym.symbols('a_1,a_2,a_3,d_4')
# リンクパラメータ
(j,a,d,alpha) = sym.symbols('j,a,d,alpha')
# T6
(n_x, n_y, n_z, o_x, o_y, o_z, a_x, a_y, a_z, p_x, p_y, p_z) = sym.symbols('n_x, n_y, n_z, o_x, o_y, o_z, a_x, a_y, a_z, p_x, p_y, p_z')
#sin.cosの簡易記述用
def S(a):
return sym.sin(a)
def C(a):
return sym.cos(a)
#回転・並進行列
def rotx(a):
return sym.Matrix([[1, 0, 0, 0], [0, C(a), -S(a), 0], [0, S(a), C(a), 0], [0, 0, 0, 1]])
def roty(a):
return sym.Matrix([[C(a), 0, S(a), 0], [0, 1, 0, 0], [-S(a), 0, C(a), 0], [0, 0, 0, 1]])
def rotz(a):
return sym.Matrix([[C(a), -S(a), 0, 0], [S(a), C(a), 0, 0], [0, 0, 1, 0], [0, 0, 0, 1]])
def trans(x, y, z):
return sym.Matrix([[1, 0, 0, x], [0, 1, 0, y], [0, 0, 1, z], [0, 0, 0, 1]])
# DH matrix
def DH(j, alpha, a, d):
return rotz(j)*trans(a,0,d)*rotx(alpha)
# inverse DH matrix
def DHi(j, alpha, a, d):
return rotx(-alpha)*trans(-a,0,-d)*rotz(-j)
| 座標系 i | Z_i-1軸回りに角度θ_i | X_i軸周りにねじれ角α_iだけ回転 | 回転後のX_i-1 (=X_i)に沿って長さa_iだけ並進 | Z_i-1に沿って距離d_iだけ並進 | |
|---|---|---|---|---|---|
| 1 | $J_1$ | $\pi/2$ | $a_1$ | 0 | |
| 2 | $J_2+\pi/2$ | 0 | $a_2$ | 0 | |
| 3 | $J_3-J_2$ | $\pi/2$ | $a_3$ | 0 | |
| 4 | $J_4$ | $-\pi/2$ | 0 | $d_4$ | |
| 5 | $J_5$ | $\pi/2$ | 0 | 0 | |
| 6 | $J_6$ | 0 | 0 | 0 | $ |
easy=False
if(easy):
A1=sym.trigsimp( DH (J_1, Pi/2, 0, 0))
A3=sym.trigsimp( DH (J_3 - J_2, Pi/2, 0, 0))
A1i=sym.trigsimp( DHi (J_1, Pi/2, 0, 0))
A3i=sym.trigsimp( DHi (J_3 - J_2, Pi/2, 0, 0))
else:
A1=sym.trigsimp( DH (J_1, Pi/2, a_1, 0))
A3=sym.trigsimp( DH (J_3 - J_2, Pi/2, a_3, 0))
# inverse matrix
A1i=sym.trigsimp( DHi (J_1, Pi/2, a_1, 0))
A3i=sym.trigsimp( DHi (J_3 - J_2, Pi/2, a_3, 0))
A2=sym.trigsimp( DH (J_2+ Pi/2, 0, a_2, 0))
A4=sym.trigsimp( DH (J_4, -Pi/2, 0, d_4))
A5=sym.trigsimp( DH (J_5, Pi/2, 0, 0))
A6=sym.trigsimp( DH (J_6, 0, 0, 0))
# inverse matrix
A2i=sym.trigsimp( DHi (J_2+ Pi/2, 0, a_2, 0))
A4i=sym.trigsimp( DHi (J_4, -Pi/2, 0, d_4))
A5i=sym.trigsimp( DHi (J_5, Pi/2, 0, 0))
A6i=sym.trigsimp( DHi (J_6, 0, 0, 0))
A1
# 逆行列をかけると単位行列になることの確認
ret = A2i*A2
sym.trigsimp(ret)
T6=sym.Matrix([[n_x, o_x, a_x, p_x], [n_y, o_y, a_y, p_y], [n_z, o_z, a_z, p_z], [0, 0, 0, 1]])
T6
# forward kinematics
A56 = sym.trigsimp( A5*A6 )
A456 = sym.trigsimp( A4*A5*A6 )
A3456 = sym.trigsimp( A3*A4*A5*A6 )
A23456 = sym.trigsimp( A2*A3*A4*A5*A6 )
T = sym.trigsimp( A1*A2*A3*A4*A5*A6 )
T
# Masahiro Furukawa
# Aug, 17, 2020
#
# refernce : https://qiita.com/JmpM/items/4bea4997aaf406cca3b4
# Cソースを得る
for ii in range(4):
for jj in range(4):
idx = jj*4+ii
code = sym.ccode(T[idx],assign_to=('Trans['+str(jj)+']['+str(ii)+']'), standard='C89')
print(code)
print()
T16 = sym.trigsimp( A1i*T6 ) # eq(3.70)
T26 = sym.trigsimp( A2i*A1i*T6 ) # eq(3.71)
T36 = sym.trigsimp( A3i*A2i*A1i*T6 ) # eq(3.72)
T46 = sym.trigsimp( A4i*A3i*A2i*A1i*T6 ) # eq(3.73)
T56 = sym.trigsimp( A5i*A4i*A3i*A2i*A1i*T6 ) # eq(3.74)
# Left hand of (3.76)
A1iT6 = T16
A1iT6
# Right hand of (3.76)
A23456
# eq2 (3.76)
sym.Eq(A1iT6[11] , A23456[11])
Therefore $$\rightarrow \displaystyle J_{1} = atan2{\left({p_{y}},{p_{x}}\right)} \\ $$
display( sym.simplify( sym.Eq(T6[11] , T[11]) ) )
print("therefore, ")
eq_C2 = ( p_z - a_3*C(J_3)-d_4*S(J_3) ) /a_2
display( sym.Eq(C(J_2), eq_C2 ) )
print("If ( 1 - (p_z - a_3 * C(J_3) - d_4 * S(J_3) )^2 < 0, then no answer.\n")
print("If ( 1 - (p_z - a_3 * C(J_3) - d_4 * S(J_3) )^2 ==0 , then ")
display( sym.Eq( - a_3 * C(J_3) - d_4 * S(J_3), 1 - p_z) )
print("Solusion on J_3 equals to the below")
sol = sym.solve( sym.Eq( - a_3 * C(J_3) - d_4 * S(J_3), 1-p_z), J_3 )
display(sym.Eq(J_3, sol[0]) ,sym.Eq(J_3, sol[1]) )
print("If ( 1 - (p_z - a_3 * C(J_3) - d_4 * S(J_3) )^2 >0 , then ")
eq_S2p = (sym.sqrt( 1 - ( p_z - a_3*C(J_3)-d_4*S(J_3) )*( p_z - a_3*C(J_3)-d_4*S(J_3) )) ) / a_3
eq_S2n = -eq_S2p
display( sym.Eq(S(J_2), eq_S2n ) )
print("Here is the equation below")
display( sym.collect( sym.Eq(A1iT6[3] , A23456[3]) , S(J_3) ) )
display( sym.collect( sym.Eq(A1iT6[7] , A23456[7]) , S(J_3) ) )
display( sym.Eq(S(J_2), eq_S2p ) )
eq3 = A23456[3] - A1iT6[3]
eq7 = A23456[7] - A1iT6[7]
print("Here is the equation below\n eq3 = ")
display( sym.collect(eq3, S(J_3) ) )
print("eq7 = ")
display( sym.collect(eq7, S(J_3) ) )
(H3, K3, A3, B3, C3, D3) = sym.symbols('H3, K3, A3, B3, C3, D3')
H3 = -a_1+p_x*C(J_1)+p_y*S(J_1)
K3 = p_z
display(H3)
eq_J3 = -a_2**2 + (-H -a_3*S(J_3)+d_4*C(J_3)) **2 + (K-a_3*C(J_3)-d_4*S(J_3)) **2
sym.collect(sym.trigsimp(sym.expand(eq_J3)), [C(J_3), S(J_3)])
# eq2 (3.76)
for idx in range(12):
display(sym.simplify ( sym.expand( sym.Eq( A1iT6[idx], A23456[idx])) ) )
# eq2 (3.76)
sym.simplify ( sym.expand( sym.Eq(
A1iT6[3]*A1iT6[3] + A1iT6[7]*A1iT6[7],
A23456[3]* A23456[3] + A23456[7]*A23456[7]) ) )
# eq2 (3.76)
sym.simplify ( sym.expand( sym.Eq(A1iT6[7]*A1iT6[7] , A23456[7]*A23456[7]) ) )
T36
A456
# eq2 (3.76)
for idx in [3,11]:
display(sym.simplify ( sym.expand( sym.Eq( T36[idx]*T36[idx], A456[idx]* A456[idx])) ) )
# eq2 (3.76)
display(sym.trigsimp ( sym.expand( sym.Eq( T36[3]*T36[3] - T36[11]*T36[11], A456[3]* A456[3]-A456[11]* A456[11])) ) )
# eq2 (3.76)
idx=6
sym.Eq(A456[idx] , T36[idx])
sym.trigsimp( A1*A2*A3*A4)
sym.trigsimp( A5i*A6i*T6)